﻿using System;
using System.Threading;
using Microsoft.SPOT;

using MLRobotic.IA.Utils;
using Gadgeteer.Modules.GHIElectronics;

namespace MLRobotic.IA
{
    public partial class Program
    {
        private ILogger _log = Log.Instance;
        private Thread _iaThread;
        private Com _com;
        private Robot _robot;
        private static DateTime _stopTime;
        private bool _matchStarted = false;

        private Gadgeteer.Color _yellow = new Gadgeteer.Color(31, 21, 0);
        private Gadgeteer.Color _red = new Gadgeteer.Color(31, 0, 0);

        // This method is run when the mainboard is powered up or reset.   
        void ProgramStarted()
        {
            Debug.Print("Program started");
            Init();
        }

        private void Init()
        {
            //Log.IsEnabled = false;
            _log.IsEnabled = System.Diagnostics.Debugger.IsAttached;

            // modules
            _robot = new Robot();
            _com = new Com(8, _robot);
            _teamLed.GreenBlueSwapped = true;

            // ... no comments
            Com.Instance = _com;
            Robot.Instance = _robot;

            // events
            _jackButton.ButtonReleased += jackButton_ButtonReleased;
            _teamButton.ButtonPressed += teamButton_ButtonPressed;

            teamButton_ButtonPressed(null, Button.ButtonState.Pressed);

            _iaThread = new Thread(new ThreadStart(MainThread));
            _iaThread.Start();
        }

        private void MainThread()
        {
            WaitJack();

            // procédure de lancement
            Log.Write("Go Go Go !!!");
            _stopTime = DateTime.Now.AddSeconds(90);

            //Thread.Sleep(500);

            _robot.Init();

            while (DateTime.Now < _stopTime)
            {
                // read captors
                _robot.UsDistance = _distance.GetDistanceInCentimeters();

                // IA loop
                _robot.Process();

                // com
                _com.Process();
            }

            _robot.Stop();
            _teamLed.TurnOff();

            Log.Write("Program terminated");

            while (true)
            {
                Thread.Sleep(100);
            }
        }

        private void WaitJack()
        {
            Log.Write("Waiting jack");

            while (!_matchStarted)
            {
                Thread.Sleep(50);
            }
        }

        private void teamButton_ButtonPressed(Button sender, Button.ButtonState state)
        {
            if (_matchStarted)
                return;

            Log.Write("Team select");

            if (_robot.Team == Robot.TeamColor.Red)
            {
                _robot.Team = Robot.TeamColor.Yellow;
                _teamLed.BlinkRepeatedly(_yellow, new TimeSpan(0, 0, 1), Gadgeteer.Color.Black, new TimeSpan(0, 0, 1));
            }
            else
            {
                _robot.Team = Robot.TeamColor.Red;
                _teamLed.BlinkRepeatedly(_red, new TimeSpan(0, 0, 1), Gadgeteer.Color.Black, new TimeSpan(0, 0, 1));
            }
        }

        void  jackButton_ButtonReleased(Button sender, Button.ButtonState state)
        {
            Log.Write("Jack detected");
            _matchStarted = true;

            if (_robot.Team == Robot.TeamColor.Red)
            {
                _teamLed.TurnColor(_red);
            }
            else
            {
                _teamLed.TurnColor(_yellow);
            }
        }
    }
}
